/*
 * pid.c
 *
 *  Created on: 2016婵°倗濮烽崕鎴﹀箯閿燂拷6闂備礁鎼悧鐐哄箯閿燂拷28闂備礁鎼崰娑㈠箯閿燂拷
 *      Author: Administrator
 */


#include "pid.h"
float KpL=4.9,KpR=5,Ki=0.015;
//-----闁喎瀹抽梻顓犲箚閹貉冨煑-----
float Speed_PI_Left(int Encoder,int Target)
{
     static float Bias,Pwm,Last_bias,Bias_Integral;
//     Kp=P; //P鐢变覆鍙ｆ帴鏀�,涓婁綅鏈鸿瀹�
//     Ki=I; //I鐢变覆鍙ｆ帴鏀�,涓婁綅鏈鸿瀹�
     Bias=Target-Encoder;
     Bias_Integral +=Bias;
     if(Bias_Integral>1)  Bias_Integral==1;
     //Pwm+=P*(Bias-Last_bias)+I*Bias;   //婢х偤鍣哄寤滻
     Pwm=KpL*Bias+Bias_Integral*Ki;   //娴ｅ秶鐤嗗寤滻
     Last_bias=Bias;                       //
     return Pwm;                         //
}

float Speed_PI_Right(int Encoder,int Target)
{
     static float Bias,Pwm,Last_bias,Bias_Integral;
//     Kp=P; //P鐢变覆鍙ｆ帴鏀�,涓婁綅鏈鸿瀹�
//     Ki=I; //I鐢变覆鍙ｆ帴鏀�,涓婁綅鏈鸿瀹�
     Bias=Target-Encoder;
     Bias_Integral +=Bias;
     if(Bias_Integral>1)  Bias_Integral==1;

     //Pwm+=Kp*(Bias-Last_bias)+Ki*Bias;   //婢х偤鍣哄寤滻
     Pwm=KpR*Bias+Bias_Integral*Ki;   //娴ｅ秶鐤嗗寤滻
     Last_bias=Bias;                       //
     return Pwm;                         //
}


//int velocity(int encoder_left)
//{
//        static float Velocity,Encoder_Least,Movement=0;
//        ki2=kp2/100;
//        Encoder_Least =Encoder_Left/25-0;
//        Encoder *= 0.8;
//        Encoder += Encoder_Least*0.2;
//        Encoder_Integral +=Encoder;
//        Encoder_Integral=Encoder_Integral-Movement;
//        if(Encoder_Integral>3000)      Encoder_Integral=3000;
//        if(Encoder_Integral<-3000) Encoder_Integral=-3000;
//        Velocity=Encoder*kp2+Encoder_Integral*ki2;
//        return Velocity;
//}


